#include "task_chassis.h"
#include "driver_chassis.h"
#include "driver_gimbal.h"

void chassis_task(void *argument) //接收+控制
{
    ChassisInit();
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
    for(;;){
        //Step1: 读取电机和功率数据
        GetWheelMotorStatus();//根据电机速度解算出当前云台坐标系速度（即逆解算出步兵此时的方向速度ChassisSpeed.SpeedX & ChassisSpeed.SpeedY）
        GetYawMotorStatus();//将所记圈数存进YawMotor.Location.Location

        //Step2: 收取控制数据
        ChassisSetSpeed(GimbalReceive.SetSpeedX,GimbalReceive.SetSpeedY);//把云台发过来的SetSpeed设进ChassisSetSpeed
        //Step3: 不同模式解算spin速度并坐标转换
        ChassisFollowCalculate(&ChassisSpeed);//锁定模式->小陀螺->跟随模式：注意这里要联合功率限制一起理解，不同的功率模式下，所设的很多速度不一样
        //Step4: 平移限速及功率环
        /*
         * 以下两部分均为限速但略有不同
         * PowerControlBySpeed函数限速是用RemoteK去限速，解决目标速度突变（突然启动或急停）时打滑（四轮加速不均衡）、可能翻车等问题
         * ChasssisPowerControl函数限速是利用功率换限速，解决底盘超功率问题
         * */
        PowerControlBySpeed(&ChassisSpeed);//用于限制平移速度
        ChassisPowerControl();//底盘功率管控，跳转可看具体方案
        //Step5: 电流值发送
        ChassisCurrentSend();//把pid算出的电流乘以合适的倍数再放进一个CAN1SendMessage再发送出去，底盘CAN1那只能是发轮电机
        osDelay(1);
    }
}
